#include <ros/ros.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <actionlib/client/action_client.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>

#include <actionlib/server/simple_action_server.h>
#include <move_arm_msgs/MoveArmAction.h>

#include <trajectory_msgs/JointTrajectory.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetPositionFK.h>

#include <motion_planning_msgs/FilterJointTrajectoryWithConstraints.h>
#include <geometric_shapes_msgs/Shape.h>
#include <motion_planning_msgs/DisplayTrajectory.h>
#include <motion_planning_msgs/GetMotionPlan.h>
#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <motion_planning_msgs/convert_messages.h>

#include <visualization_msgs/Marker.h>

#include <planning_environment/util/construct_object.h>
#include <planning_environment/monitors/joint_state_monitor.h>
#include <geometric_shapes/bodies.h>

#include <planning_environment_msgs/GetRobotState.h>
#include <planning_environment_msgs/GetJointTrajectoryValidity.h>
#include <planning_environment_msgs/GetStateValidity.h>
#include <planning_environment_msgs/GetJointsInGroup.h>
#include <planning_environment_msgs/GetEnvironmentSafety.h>
#include <planning_environment_msgs/SetConstraints.h>

#include <visualization_msgs/MarkerArray.h>

#include <std_msgs/Bool.h>

#include <valarray>
#include <algorithm>
#include <cstdlib>

namespace pingpang_move_arm
{

const std::string ARM_IK_NAME = "arm_ik";

class PingpangMoveArm
{
private:

	std::string group_;
	ros::NodeHandle root_handle_, private_handle_;

    ros::ServiceClient fk_client_;
	ros::ServiceClient ik_client_;
	ros::ServiceClient get_joints_in_group_client_, get_state_client_;
	ros::ServiceClient planning_client_;

	std::string planner_service_name_;

	motion_planning_msgs::GetMotionPlan::Request original_request_;

    trajectory_msgs::JointTrajectory current_trajectory_;

	boost::shared_ptr<actionlib::SimpleActionServer<move_arm_msgs::MoveArmAction> > action_server_;
	move_arm_msgs::MoveArmFeedback move_arm_action_feedback_;
	move_arm_msgs::MoveArmResult move_arm_action_result_;

	planning_environment::JointStateMonitor state_monitor_;
	std::vector<std::string> group_joint_names_;




	bool arm_ik_initialized_;
	double ik_allowed_time_;
	double move_arm_frequency_;

public:

	PingpangMoveArm(const std::string group_name): group_(group_name), private_handle_("~")
	{
	    private_handle_.param<double>("ik_allowed_time", ik_allowed_time_, 2.0);
	    private_handle_.param<double>("move_arm_frequency", move_arm_frequency_, 50.0);


		ik_client_ = root_handle_.serviceClient<kinematics_msgs::GetPositionIK>(ARM_IK_NAME);
		get_joints_in_group_client_ = root_handle_.serviceClient<planning_environment_msgs::GetJointsInGroup>("get_joints_in_group");
	    get_state_client_ = root_handle_.serviceClient<planning_environment_msgs::GetRobotState>("get_robot_state");

	    arm_ik_initialized_ = false;

	    ros::service::waitForService("get_joints_in_group");
	    ros::service::waitForService("get_robot_state");

	    action_server_.reset(new actionlib::SimpleActionServer<move_arm_msgs::MoveArmAction>(root_handle_, "pingpang_move_" + group_name, boost::bind(&PingpangMoveArm::executeCB, this, _1)));
	}

	virtual ~PingpangMoveArm()
	{

	}

	bool configure()
	{
		if (!state_monitor_.active_)
		{
			ROS_ERROR("Joint state monitor is not active, aborting.");
			return false;
		}

		if (group_.empty())
		{
			ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
			return false;
		}
		else
		{
			planning_environment_msgs::GetJointsInGroup::Request request;
			planning_environment_msgs::GetJointsInGroup::Response response;
			request.group_name = group_;
			if (get_joints_in_group_client_.call(request, response))
			{
				if (!response.joint_names.empty())
				{
					group_joint_names_ = response.joint_names;
					return true;
				}
				else
				{
					ROS_ERROR("Could not get the list of joint names in the group: %s", group_.c_str());
					return false;
				}
			}
			else
			{
				ROS_ERROR("Service call to find list of joint names failed on %s", get_joints_in_group_client_.getService().c_str());
				return false;
			}
		}
		return true;
	}

private:

	///
	///	Kinematics
	///
	bool convertPoseGoalToJointGoal(motion_planning_msgs::GetMotionPlan::Request &request)
	{
		if(!arm_ik_initialized_)
		{
			if(!ros::service::waitForService(ARM_IK_NAME))
			{
				ROS_WARN("Inverse kinematics service is unavailable");
				return false;
			}
			else
			{
				arm_ik_initialized_ = true;
			}
		}

		ROS_DEBUG("Acting on goal to pose ...");// we do IK to find corresponding states
		ROS_DEBUG("Position constraint: %f %f %f",
				request.motion_plan_request.goal_constraints.position_constraints[0].position.x,
				request.motion_plan_request.goal_constraints.position_constraints[0].position.y,
				request.motion_plan_request.goal_constraints.position_constraints[0].position.z);
		ROS_DEBUG("Orientation constraint: %f %f %f %f",
				request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x,
				request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y,
				request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z,
				request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w);

		geometry_msgs::PoseStamped_ desired_pose = motion_planning_msgs::poseConstraintsToPoseStamped(
																					     request.motion_plan_request.goal_constraints.position_constraints[0],
																						 request.motion_plan_request.goal_constraints.orientation_constraints[0]);
	    std::string link_name = request.motion_plan_request.goal_constraints.position_constraints[0].link_name;

	    ROS_INFO("convertPoseGoalToJointGoal: \nIK request");
	    ROS_INFO("link_name   : %s",desired_pose.header.frame_id.c_str());
		ROS_INFO("frame_id    : %s",desired_pose.header.frame_id.c_str());
		ROS_INFO("position    : (%f,%f,%f)",desired_pose.pose.position.x,desired_pose.pose.position.y,desired_pose.pose.position.z);
		ROS_INFO("orientation : (%f,%f,%f,%f)", desired_pose.pose.orientation.x,desired_pose.pose.orientation.y,desired_pose.pose.orientation.z,desired_pose.pose.orientation.w);
		ROS_INFO(" ");

	    sensor_msgs::JointState joints_solution;

	    if (computeIK(desired_pose, link_name, joints_solution))
	    {
	      if(!checkIK(desired_pose, link_name, joints_solution))
	      {
	         ROS_ERROR("IK solution does not get to desired pose");
	      }

	      for (unsigned int i = 0 ; i < joints_solution.name.size() ; ++i)
	      {
	        motion_planning_msgs::JointConstraint jc;
	        jc.joint_name = joints_solution.name[i];
	        jc.position = joints_solution.position[i];
	        jc.tolerance_below = 0.1;
	        jc.tolerance_above = 0.1;
	        request.motion_plan_request.goal_constraints.joint_constraints.push_back(jc);
	      }
	      request.motion_plan_request.goal_constraints.position_constraints.clear();
	      request.motion_plan_request.goal_constraints.orientation_constraints.clear();
	      return true;
	    }
	    else
	      return false;
	}

	bool computeIK(const geometry_msgs::PoseStamped_ &desired_pose, const std::string &link_name, sensor_msgs::JointState &joints_solution)
	{
		kinematics_msgs::GetPositionIK::Request IK_request;
		kinematics_msgs::GetPositionIK::Response IK_response;

		IK_request.ik_request.pose_stamped = desired_pose;
	    IK_request.ik_request.ik_seed_state.joint_state.name = group_joint_names_;
	    IK_request.ik_request.ik_link_name = link_name;
	    IK_request.ik_request.ik_seed_state.joint_state = state_monitor_.getJointState(group_joint_names_);
	    IK_request.timeout = ros::Duration(ik_allowed_time_);

	    if (ik_client_.call(IK_request, IK_response))
	    {
	       move_arm_action_result_.error_code = IK_response.error_code;
	       if(IK_response.error_code.val != IK_response.error_code.SUCCESS)
	       {
	         ROS_ERROR("IK Solution not found, IK returned with error_code: %d",IK_response.error_code.val);
	         return false;
	       }
	       joints_solution = IK_response.solution.joint_state;
	       if (joints_solution.position.size() != IK_request.ik_request.ik_seed_state.joint_state.name.size())
	       {
	         ROS_ERROR("Incorrect number of elements in IK output.");
	         return false;
	       }
	       for(unsigned int i = 0; i < joints_solution.position.size() ; ++i)
	         ROS_DEBUG("IK[%d] = %f", (int)i, joints_solution.position[i]);
	     }
	     else
	     {
	       ROS_ERROR("IK service failed");
	       return false;
	     }
	     return true;
	}

	bool checkIK(const geometry_msgs::PoseStamped_ &pose_stamped_msg, const std::string &link_name, sensor_msgs::JointState &joints_solution)
	{
		kinematics_msgs::GetPositionFK::Request FK_request;
		kinematics_msgs::GetPositionFK::Response FK_response;

		FK_request.robot_state.joint_state.name = group_joint_names_;
		FK_request.fk_link_names.resize(1);
		FK_request.fk_link_names[0] = link_name;
		FK_request.robot_state.joint_state.position = joints_solution.position;
		FK_request.header = pose_stamped_msg.header;
		if (fk_client_.call(FK_request, FK_response))
		{
			if (FK_response.error_code.val != FK_response.error_code.SUCCESS)
				return false;
			ROS_DEBUG("Obtained FK solution");
			ROS_DEBUG("FK Pose:");
			ROS_DEBUG("Position : (%f,%f,%f)",
					FK_response.pose_stamped[0].pose.position.x,
					FK_response.pose_stamped[0].pose.position.y,
					FK_response.pose_stamped[0].pose.position.z);
			ROS_DEBUG("Rotation : (%f,%f,%f,%f)",
					FK_response.pose_stamped[0].pose.orientation.x,
					FK_response.pose_stamped[0].pose.orientation.y,
					FK_response.pose_stamped[0].pose.orientation.z,
					FK_response.pose_stamped[0].pose.orientation.w);
			ROS_DEBUG(" ");
		}
		else
		{
			ROS_ERROR("FK service failed");
			return false;
		}
		return true;
	}
	///
	/// End Kinematics
	///

	///
	/// Motion planning
	///

	bool moveArmGoalToPlanerRequest(const move_arm_msgs::MoveArmGoalConstPtr &goal, motion_planning_msgs::GetMotionPlan::Request &plan_request)
	{
		plan_request.motion_plan_request = goal->motion_plan_request;
		planner_service_name_ = goal->planner_service_name;
		return true;
	}

	bool createPlan(motion_planning_msgs::GetMotionPlan::Request &request, motion_planning_msgs::GetMotionPlan::Response &response)
	{
		if(!getRobotState(request.motion_plan_request.start_state))
			return false;
        planning_client_ = ros::ServiceClient<motion_planning_msgs::GetMotionPlan>(planner_service_name_);
        ROS_DEBUG("Issuing request for motion plan.");

        if(planning_client_.call(request, response))
        {
        	if(response.trajectory.joint_trajectory.points.empty())
        	{
                ROS_ERROR("Motion planner was unable to plan a path to goal");
                return false;
        	}
            ROS_INFO("Motion planning succeeded");
/*
            motion_planning_msgs::RobotState last_state_in_trajectory;
            last_state_in_trajectory.joint_state.header = response.trajectory.joint_trajectory.header;
            last_state_in_trajectory.joint_state.name = response.trajectory.joint_trajectory.joint_names;
            last_state_in_trajectory.joint_state.position = response.trajectory.joint_trajectory.points.back().positions;
*/
        }
        else
        {
          ROS_ERROR("Motion planning service failed on %s",planning_client_.getService().c_str());
          return;
        }
		return true;
	}

	///
	/// Helper functions
	///
	bool isPoseGoal(motion_planning_msgs::GetMotionPlan::Request &req)
	{
		if (req.motion_plan_request.goal_constraints.joint_constraints.empty()
				&& // we have no joint constraints on the goal,
				req.motion_plan_request.goal_constraints.position_constraints.size()
						== 1 && // we have a single position constraint on the goal
				req.motion_plan_request.goal_constraints.orientation_constraints.size()
						== 1) // that is active on all 6 DOFs
			return true;
		else
			return false;
	}

	bool isJointGoal(motion_planning_msgs::GetMotionPlan::Request &req)
	{
		if (req.motion_plan_request.goal_constraints.position_constraints.empty()
				&& req.motion_plan_request.goal_constraints.orientation_constraints.empty()
				&& !req.motion_plan_request.goal_constraints.joint_constraints.empty())
			return true;
		else
			return false;
	}

	bool getRobotState(motion_planning_msgs::RobotState &robot_state)
	{
		planning_environment_msgs::GetRobotState::Request request;
		planning_environment_msgs::GetRobotState::Response response;
		if(get_state_client_.call(request, response))
		{
			robot_state = response.robot_state;
			return true;
		}
		else
		{
			ROS_ERROR("Service call to get robot state failed on %s", get_state_client_.getService().c_str());
			return false;
		}

	}
	///
	///	Ender Helper functions
	///


	void executeCB(const move_arm_msgs::MoveArmGoalConstPtr &goal)
	{
		motion_planning_msgs::GetMotionPlan::Request plan_request;
		motion_planning_msgs::GetMotionPlan::Response plan_response;

		moveArmGoalToPlanerRequest(goal, plan_request);
		original_request_ = plan_request;

		ROS_INFO("Received new goal");

	    move_arm_action_result_.contacts.clear();
	    move_arm_action_result_.error_code.val = 0;

//	    ros::Rate move_arm_rate(move_arm_frequency_);
//	    while(private_handle_.ok())

        if (action_server_->isPreemptRequested())
		{
			if (action_server_->isNewGoalAvailable())
			{
				move_arm_action_result_.contacts.clear();
				move_arm_action_result_.error_code.val = 0;
				moveArmGoalToPlanerRequest(
						(action_server_->acceptNewGoal()), plan_request);
				ROS_INFO("Received new goal, will preempt previous goal");
				original_request_ = plan_request;
			}
			else //if we've been preempted explicitly we need to shut things down
			{
				ROS_INFO(
						"The move arm action was preempted by the action client. Preempting this goal.");
				action_server_->setPreempted();
				return;
			}
		}

		ros::WallTime start = ros::WallTime::now();

		move_arm_action_feedback_.state = "planning";
		move_arm_action_feedback_.time_to_completion = ros::Duration(
				plan_request.motion_plan_request.allowed_planning_time);

		action_server_->publishFeedback(move_arm_action_feedback_);

		if (isPoseGoal(plan_request))
		{
			ROS_INFO("Planning to a pose goal");
			if (!convertPoseGoalToJointGoal(plan_request))
			{
				action_server_->setAborted(move_arm_action_result_);
				return;
			}
		}
		else if (isJointGoal(plan_request))
		{
			ROS_INFO("Planning to a joint goal");
		}

//	        visualizeJointGoal(plan_request);

		if(createPlan(plan_request, plan_response))
		{
			current_trajectory_ = plan_response.trajectory.joint_trajectory;
//	        visualizePlan(current_trajectory_);
			move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS;
			action_server_->setSucceeded(move_arm_action_result_);
			ROS_INFO("pingpang_move arm Succeeded");
			//Todo publish trajectory
		}
		else
		{
			ROS_ERROR("Create plan failed");
		}

	}




};





}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "move_arm");

	ros::AsyncSpinner spinner(1);
	spinner.start();

	ros::NodeHandle nh("~");
	std::string group;

	nh.param<std::string>("group", group, std::string);
	ROS_INFO("Move arm operating on group %s", group.c_str());


	return 0;
}
